#include "esp301-control.h"
#include <sstream>

#if defined (_WIN32) || defined( _WIN64)
#define DEVICE_PORT "COM1"                               // COM1 for windows
#elif defined (__linux__)
#define DEVICE_PORT "/dev/ttyS0"                         // ttyS0 for linux
#endif

#define BAUDRATE 19200
#define TIMEOUT 10000
#define STEADY_STATE_WAIT 200

esp301_control::esp301_control()
{
  theoretical_displacement = 0;
  connect();
}

void esp301_control::calibrate()
{
  goHome();
  setLimit();
}

bool esp301_control::connect()
{
  ret=device.Open(DEVICE_PORT,BAUDRATE);                                        // Open serial link at 115200 bauds
  if (ret!=1)                                                             // If an error occured...
    throw "Error while opening port. Permission problem ?";        // ... display a message ...

  ret=device.WriteString("1MO;TB\r");
  if (ret!=1)
    {
      throw "Cannot connect to device. Check whether the cable is connected, and turn on the ESP301 controller.";
      return 0;
    }
  ret=device.ReadString(buffer,'\r',RS232_BUFFER_MAX,TIMEOUT);                                // Read a maximum of 128 characters with a timeout of 0.3 second
  // The final character of the string must be a line feed ('\n')

  if (ret == 0)
    {
      throw "Cannot identify device ID. Restart the motion controller ESP301";                    // If not, print a message.
      return 0;
    }
  return 1;
}

void esp301_control::goHome()
{
  ret=device.WriteString("1OR;1WS0\r");
}

void esp301_control::setLimit()
{
  std::stringstream format_instruction;
  format_instruction << "1SL" << ESP301_LOWER_LIMIT << ";1SR" << ESP301_UPPER_LIMIT << "\r";

  ret=device.WriteString(format_instruction.str().c_str());

  chkError();
}

esp301_control::~esp301_control()
{
//ret=device.WriteString("1MF\r");
  device.Close();
}

float esp301_control::getPosition()
{

  ret=device.WriteString("1WS0;1TP\r");                                             // Send the command on the serial port
  if (ret!=1)                                                             // If the writting operation failed ...
    throw "Error sending command (1TP)";                              // ... display a message ...

  // Read a string from the serial device
  ret=device.ReadString(buffer,'\r',RS232_BUFFER_MAX,TIMEOUT);                                // Read a maximum of 128 characters with a timeout of 0.3 second
  // The final character of the string must be a line feed ('\n')

  if (ret == 0)
    throw "TimeOut reached. No data received !";                    // If not, print a message.

  std::stringstream format_decimal (buffer);

  float displacement;
  format_decimal >> displacement;
  return displacement;
}

void esp301_control::setTolerance(float tol)
{
  std::stringstream format_decimal;
  format_decimal << "1FE" << tol << "\r";

  ret=device.WriteString(format_decimal.str().c_str());                                             // Send the command on the serial port
  if (ret!=1)                                                             // If the writting operation failed ...
    throw "Error sending command (1FE*)";                              // ... display a message ...

  chkError();
}


bool esp301_control::moveTo(double x)
{
  if (x < ESP301_LOWER_LIMIT || x > ESP301_UPPER_LIMIT)
    {
      throw "Requested position out of hardware range";
      return 0;
    }

  std::stringstream format_instruction;
  format_instruction << "1PA" << x << ";1WS" << STEADY_STATE_WAIT << "\r";

  ret=device.WriteString(format_instruction.str().c_str());
  if (ret!=1)
    throw "Cannot send command (1PA*)";

  chkError();

  theoretical_displacement = x;
  return 1;
}

void esp301_control::chkError()
{

  ret=device.WriteString("TB\r");
  if (ret!=1)
    {
      throw "Cannot send command (TB)";
      return;
    }

  ret=device.ReadString(buffer,'\r',RS232_BUFFER_MAX,TIMEOUT);                                // Read a maximum of 128 characters with a timeout of 0.3 second
  // The final character of the string must be a line feed ('\n')

  if (ret == 0)
    throw "TimeOut reached. No data received! (Expect 0)";                    // If not, print a message.

  if (buffer[1] != '0')
    throw (buffer+1);
}

bool esp301_control::moveBy(double dx)
{

  return moveTo(theoretical_displacement + dx);

}





